#include <Windows.h>
#include <NuiApi.h>
#define DEBUG_MODE

using namespace std;
ULONGLONG GetTime()
{
	return GetTickCount64();
}

enum GestureType
{
	UNKNOWN_GESTURE_TYPE = 0,
	LEFT_HAND_SWIPE_RIGHT = 1,
	RIGHT_HAND_SWIPE_LEFT = 2,
	COUNT = 3,
};

class GestureRecognizer
{
	bool isStarted;
	ULONGLONG gestureStartTime;
public:
	GestureRecognizer() { isStarted = false; lastGestureCapturedTime = 0; gestureStartTime = 0; };
	ULONGLONG lastGestureCapturedTime;
	GestureType gestureType;
	void CheckGesture (NUI_SKELETON_DATA skelData);
	bool IsNew();
};

bool GestureRecognizer::IsNew()
{
	if(GetTime() - lastGestureCapturedTime < 1000 )
	{
		lastGestureCapturedTime = 0;
		return true;
	}
	return false;
}

void GestureRecognizer::CheckGesture(NUI_SKELETON_DATA skelData)
{
	float leftX  = skelData.SkeletonPositions[NUI_SKELETON_POSITION_HAND_LEFT].x - skelData.SkeletonPositions[NUI_SKELETON_POSITION_HIP_CENTER].x;
	float leftY  = skelData.SkeletonPositions[NUI_SKELETON_POSITION_HAND_LEFT].y - skelData.SkeletonPositions[NUI_SKELETON_POSITION_HIP_CENTER].y;
	float rightX = skelData.SkeletonPositions[NUI_SKELETON_POSITION_HAND_RIGHT].x - skelData.SkeletonPositions[NUI_SKELETON_POSITION_HIP_CENTER].x;
	float rightY = skelData.SkeletonPositions[NUI_SKELETON_POSITION_HAND_RIGHT].y - skelData.SkeletonPositions[NUI_SKELETON_POSITION_HIP_CENTER].y;

	leftX  /= skelData.SkeletonPositions[NUI_SKELETON_POSITION_HIP_CENTER].z;
	leftY  /= skelData.SkeletonPositions[NUI_SKELETON_POSITION_HIP_CENTER].z;
	rightX /= skelData.SkeletonPositions[NUI_SKELETON_POSITION_HIP_CENTER].z;
	rightY /= skelData.SkeletonPositions[NUI_SKELETON_POSITION_HIP_CENTER].z;

	float hipCenterInitialX=skelData.SkeletonPositions[NUI_SKELETON_POSITION_HIP_CENTER].x;
	float hipCenterDeltaX;

	float hipCenterInitialZ=skelData.SkeletonPositions[NUI_SKELETON_POSITION_HIP_CENTER].z;
	float hipCenterDeltaZ;


	if(GetTime() - gestureStartTime < 2000) 
	{
		hipCenterDeltaX = abs(skelData.SkeletonPositions[NUI_SKELETON_POSITION_HIP_CENTER].x - hipCenterInitialX); // keeps track of the change in hipCenter's x position
		hipCenterDeltaZ = abs(skelData.SkeletonPositions[NUI_SKELETON_POSITION_HIP_CENTER].z - hipCenterInitialZ);
		if(gestureType == GestureType::LEFT_HAND_SWIPE_RIGHT)
		{
			if((abs( leftY ) < 0.2 && leftX > 0.1) && hipCenterDeltaX<0.5 && hipCenterDeltaZ<0.5 ) // The initals were leftY=0.2 and leftX=0.2 and the second part after && is added
			{
				lastGestureCapturedTime = GetTime();
			}
		}
		else if(gestureType == GestureType::RIGHT_HAND_SWIPE_LEFT)
		{
			if((abs(rightY) < 0.2 && rightX < -0.1) && hipCenterDeltaX<0.5 && hipCenterDeltaZ<0.5 ) // the second part after && is added
			{
				lastGestureCapturedTime = GetTime();
			}
		}
	}
	else
	{
		if(abs(rightY) < 0.2 && rightX > 0.3) // The initals were 0.2 for rightY and 0.3 for rightX
		{
			hipCenterInitialX = skelData.SkeletonPositions[NUI_SKELETON_POSITION_HIP_CENTER].x; // added for more robust gesture rec.
			hipCenterInitialZ = skelData.SkeletonPositions[NUI_SKELETON_POSITION_HIP_CENTER].z; // added for more robust gesture rec.
			gestureStartTime = GetTime();
			gestureType = GestureType::RIGHT_HAND_SWIPE_LEFT;
		}
		else if(abs(leftY) < 0.2 && leftX < -0.3) // The initals were 0.2 for leftY and -0.3 for leftX
		{
			hipCenterInitialX = skelData.SkeletonPositions[NUI_SKELETON_POSITION_HIP_CENTER].x; //added for more robust gesture rec.
			hipCenterInitialZ = skelData.SkeletonPositions[NUI_SKELETON_POSITION_HIP_CENTER].z; // added for more robust gesture rec.
			gestureStartTime = GetTime();
			gestureType = GestureType::LEFT_HAND_SWIPE_RIGHT;
		}
	}
}

struct Coordinate
{
public:
	float X;
	float Z;
	ULONGLONG timeStamp;

	Coordinate()
	{
		timeStamp = GetTime();
	}
};

void SendM (char* message)
{
#ifdef DEBUG_MODE
	std::cout<<message<<std::endl;
#endif
}

class KinectDev 
{
public:
	KinectDev();
	HRESULT Init ();
	HRESULT Init( OLECHAR *instanceName );
	HRESULT UnInit();
	~KinectDev();

	void Update();

	INuiSensor* kinectSensor;
	
	HANDLE skeletonThrd;
	HANDLE nuiProcessThrd;
	HANDLE nuiProcessStopThrd;
	HRESULT ProcessSkeleton();
	
	void DefaultSet();
	DWORD WINAPI Nui_ProcessThread();
	static DWORD WINAPI  Nui_ProcessThread(LPVOID pParam);
	
	NUI_SKELETON_FRAME*		m_skeletonFrame;	
	Coordinate lastCoordinate;	
	void SendP(Vector4 pos); // Invokes when new skeleton data arrives ...
	void SkeletonInvoke ( NUI_SKELETON_DATA skeletonData);
	
	GestureRecognizer gestureRecognizer;

	static int trackedSkeletonID;// For keeping track of skeletons

};

int KinectDev::trackedSkeletonID = -1;
KinectDev::KinectDev() { gestureRecognizer = GestureRecognizer(); }
KinectDev::~KinectDev() { }

HRESULT KinectDev::UnInit()
{
	// Stop the Nui processing thread
	if ( NULL != nuiProcessStopThrd )
	{
		// Signal the thread
		SetEvent(nuiProcessStopThrd);

		// Wait for thread to stop
		if ( NULL != nuiProcessThrd )
		{
			WaitForSingleObject( nuiProcessThrd, INFINITE );
			CloseHandle( nuiProcessThrd );
		}
		CloseHandle( nuiProcessStopThrd );
	}
	if ( kinectSensor )
	{
		kinectSensor->NuiShutdown( );
	}
	if ( skeletonThrd && ( skeletonThrd != INVALID_HANDLE_VALUE ) )
	{
		CloseHandle( skeletonThrd );
		skeletonThrd = NULL;
	}
	if ( kinectSensor )
	{
		kinectSensor->Release();
		kinectSensor = NULL;
	}  
	return S_OK;
}
HRESULT KinectDev::Init( OLECHAR *instanceName )
{
	//trackedSkeletonID = -1; // Init of tracked skeleton ID
	// Generic creation failure
	if ( NULL == instanceName )
	{
		SendM("ERROR_NULL_INSTANCE_NAME");
		return E_FAIL;
	}

	HRESULT hr = NuiCreateSensorById( instanceName, &kinectSensor );
	
	if ( FAILED(hr) )
	{
		SendM("ERROR_NO_DEVICE");
		return hr;
	}

	return Init();
}

HRESULT KinectDev::Init()
{
	HRESULT hr = NuiCreateSensorByIndex(0, &kinectSensor);
	if (FAILED(hr) )
    {
		SendM ("ERROR_NO_DEVICE");
        return hr;
    }

	skeletonThrd = CreateEventW(NULL, TRUE, FALSE, NULL);
	hr = kinectSensor->NuiInitialize( NUI_INITIALIZE_FLAG_USES_SKELETON );  
	if ( FAILED( hr ) )
	{
		SendM (  E_NUI_DEVICE_IN_USE == hr ? "ERROR_IN_USE" : "ERROR_NUIINIT");
		return hr;
	}

	if ( HasSkeletalEngine( kinectSensor ) )
	{
		hr = kinectSensor->NuiSkeletonTrackingEnable( skeletonThrd, 0 );
		if( FAILED( hr ) )
		{
			SendM("ERROR_SKELETON_TRACKING_1");
			return hr;
		}
	}

	hr = kinectSensor->NuiSkeletonTrackingEnable(skeletonThrd, 0);
	if (FAILED(hr))
	{
		SendM("ERROR_SKELETON_FRAME");
		return hr;
	}

	nuiProcessStopThrd = CreateEvent( NULL, FALSE, FALSE, NULL );
	nuiProcessThrd = CreateThread( NULL, 0, Nui_ProcessThread, this, 0, NULL );

	SendM ("NOERROR");
	return S_OK;
}

DWORD WINAPI KinectDev::Nui_ProcessThread(LPVOID pParam)
{
	KinectDev *pthis = (KinectDev *) pParam;
	return pthis->Nui_ProcessThread();
}

DWORD WINAPI KinectDev::Nui_ProcessThread()
{
	HANDLE hEvents[] =  {nuiProcessStopThrd, skeletonThrd };
	int    nEventIdx;

	// Main thread loop
	bool continueProcessing = true;
	while ( continueProcessing )
	{
		nEventIdx = WaitForMultipleObjects( 2, hEvents, FALSE, 100 );

		switch ( nEventIdx )
		{
			case WAIT_TIMEOUT:
				continue;

			// If the stop event, stop looping and exit
			case WAIT_OBJECT_0:
				continueProcessing = false;
				continue;

			default:
				Update();
				break;
		}
	}

	return 0;
}

HRESULT KinectDev::ProcessSkeleton()
{
    NUI_SKELETON_FRAME skeletonFrame = {0};

	HRESULT hr;
    hr = kinectSensor->NuiSkeletonGetNextFrame(0, &skeletonFrame);

	//even if next frame fails, m_skeletonFrame must be assign to empty skeleton frame
	m_skeletonFrame = new NUI_SKELETON_FRAME(skeletonFrame);

    if ( FAILED(hr) )
    {
		SendM("ERROR_DEPTH_STREAM");
        return hr;
    }

    // smooth out the skeleton data
    hr = kinectSensor -> NuiTransformSmooth(&skeletonFrame, NULL);
	if ( FAILED(hr) )
    {
		SendM("ERROR_SMOOTING_FAILED");
        return hr;
    }
	return hr;
}

void KinectDev::SendP(Vector4 pos)
{
	lastCoordinate.X = pos.x;
	lastCoordinate.Z = pos.z;
	lastCoordinate.timeStamp = GetTime();
}

void KinectDev::SkeletonInvoke ( NUI_SKELETON_DATA skeletonData )
{
	// float x = skeletonData->SkeletonPositions[NUI_SKELETON_POSITION_ANKLE_LEFT].x;

	Vector4 pos = skeletonData.Position;
	SendP(pos);

	gestureRecognizer.CheckGesture(skeletonData);
	
	
}

void KinectDev::Update()
{
	if (NULL == kinectSensor)
    {
		SendM("ERROR_NO_DEVICE");
        return;
    }

	// if we have received any valid new skeleton data we may need to draw

        if ( !SUCCEEDED(ProcessSkeleton()) )
            return;

	float minZ = 1000;
	int minIdx = -1;

	if(trackedSkeletonID==-1)
	{
		// the original code portion begins
		for (int i=0; i<6; i++) // pick the closest skeleton
		{
			if (m_skeletonFrame->SkeletonData[i].eTrackingState == NUI_SKELETON_POSITION_TRACKED)
			{
				if(m_skeletonFrame->SkeletonData[i].Position.z < minZ)
				{
					minZ = m_skeletonFrame->SkeletonData[i].Position.z;
					minIdx = i;
					cout<<"Skeletonu gordum"<<endl;
				}
			}
		}
		//the original code portion ends
		trackedSkeletonID = minIdx;
	}
	else if(m_skeletonFrame->SkeletonData[trackedSkeletonID].eTrackingState != NUI_SKELETON_NOT_TRACKED)
	{
		SkeletonInvoke( m_skeletonFrame -> SkeletonData[trackedSkeletonID] );
	}
	else
	{
		trackedSkeletonID = -1;
		return;
	}
			
	//if( minIdx == -1 ) // no skeleton
	//	return;

	//SkeletonInvoke( m_skeletonFrame -> SkeletonData[minIdx] );
}

